#include <firmament.h>
#include "hal/actuator/actuator.h"

#include "device.h"
#include "f28p65x_device.h"
#include "drv_pwm.h"

// #define DRV_DBG(...) console_printf(__VA_ARGS__)
#define DRV_DBG(...)

#define PWM_FREQ_50HZ  (50)
#define PWM_FREQ_125HZ (125)
#define PWM_FREQ_250HZ (250)
#define PWM_FREQ_400HZ (400)

#define MAX_PWM_OUT_CHAN      10            // Main Out has 10 pwm channel
#define TIMER_FREQUENCY       2500000       // Timer frequency: 2.5M
#define PWM_DEFAULT_FREQUENCY PWM_FREQ_50HZ // pwm default frequqncy
#define VAL_TO_DC(_val)       ((float)(_val * __pwm_freq) / 1000000.0f)
#define DC_TO_VAL(_dc)        (1000000.0f / __pwm_freq * _dc)

#define PWM_ARR(freq) (TIMER_FREQUENCY / freq) // CCR reload value, Timer frequency = TIMER_FREQUENCY/(PWM_ARR+1)

static rt_err_t pwm_config(actuator_dev_t dev, const struct actuator_configure* cfg);
static rt_err_t pwm_control(actuator_dev_t dev, int cmd, void* arg);
static rt_size_t pwm_read(actuator_dev_t dev, rt_uint16_t chan_sel, rt_uint16_t* chan_val, rt_size_t size);
static rt_size_t pwm_write(actuator_dev_t dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size);

const static struct actuator_ops __act_ops = {
    .act_config = pwm_config,
    .act_control = pwm_control,
    .act_read = pwm_read,
    .act_write = pwm_write
};

static struct actuator_device act_dev = {
    .chan_mask = 0x3FF,
    .range = { 1000, 2000 },
    .config = {
        .protocol = ACT_PROTOCOL_PWM,
        .chan_num = MAX_PWM_OUT_CHAN,
        .pwm_config = { .pwm_freq = 50 },
        .dshot_config = { 0 } },
    .ops = &__act_ops
};

static uint32_t __pwm_freq = PWM_DEFAULT_FREQUENCY;
static float __pwm_dc[MAX_PWM_OUT_CHAN];

/*----------------------------------------------------------------------------------*/
static struct qx_pwm qx_pwm_obj[] = {
#if defined(BOARD_CFG_PWM1)
        PMW1_CONFIG,
#endif

#if defined(BOARD_CFG_PWM2)
        PMW2_CONFIG,
#endif

#if defined(BOARD_CFG_PWM3)
        PMW3_CONFIG,
#endif

#if defined(BOARD_CFG_PWM4)
        PMW4_CONFIG,
#endif

#if defined(BOARD_CFG_PWM5)
        PMW5_CONFIG,
#endif

#if defined(BOARD_CFG_PWM6)
        PMW6_CONFIG,
#endif

#if defined(BOARD_CFG_PWM7)
        PMW7_CONFIG,
#endif

#if defined(BOARD_CFG_PWM8)
        PMW8_CONFIG,
#endif

#if defined(BOARD_CFG_PWM9)
        PMW9_CONFIG,
#endif

#if defined(BOARD_CFG_PWM10)
        PMW10_CONFIG,
#endif

#if defined(BOARD_CFG_PWM11)
        PMW11_CONFIG,
#endif

#if defined(BOARD_CFG_PWM12)
        PMW12_CONFIG,
#endif
};

const rt_uint8_t pwm_output_io_support_list[31] = {
        0,//GPIO_0
        1,//GPIO_1
        2,//GPIO_2
        3,//GPIO_3
        4,//GPIO_4
        5,//GPIO_5
        6,//GPIO_6
        7,//GPIO_7
        8,//GPIO_8
        9,//GPIO_9
        10,//GPIO_10
        11,//GPIO_11
        12,//GPIO_12
        13,//GPIO_13
        14,//GPIO_14
        15,//GPIO_15
        16,//GPIO_16
        17,//GPIO_17
        24,//GPIO_24
        28,//GPIO_28
        29,//GPIO_29
        30,//GPIO_30
        31,//GPIO_31
        32,//GPIO_32
        39,//GPIO_39
        40,//GPIO_40
        41,//GPIO_41
        56,//GPIO_56
        57,//GPIO_57
        58,//GPIO_58
        59,//GPIO_59
};

const rt_uint32_t pwm_io_mux_value_list[31][1] = {
        {GPIO_0_EPWM1_A},
        {GPIO_1_EPWM1_B},
        {GPIO_2_EPWM2_A},
        {GPIO_3_EPWM2_B},
        {GPIO_4_EPWM3_A},
        {GPIO_5_EPWM3_B},
        {GPIO_6_EPWM4_A},
        {GPIO_7_EPWM4_B},
        {GPIO_8_EPWM5_A},
        {GPIO_9_EPWM5_B},
        {GPIO_10_EPWM6_A},
        {GPIO_11_EPWM6_B},
        {GPIO_12_EPWM7_A},
        {GPIO_13_EPWM7_B},
        {GPIO_14_EPWM8_A},
        {GPIO_15_EPWM8_B},
        {GPIO_16_EPWM9_A},
        {GPIO_17_EPWM9_B},
        {GPIO_18_EPWM10_A},
        {GPIO_19_EPWM10_B},
};

static rt_err_t qx_pwm_support_io_check(rt_uint8_t register_io)
{
    rt_err_t rst = RT_ERROR;

    for(rt_uint8_t i = 0; i < sizeof(pwm_output_io_support_list); i++){
        if(pwm_output_io_support_list[i] == register_io){
            rst = RT_EOK;
            break;
        }
    }

    return rst;
}

static void qx_pwm_io_config(rt_uint8_t register_io)
{
    rt_uint32_t io_mux = 0xffUL;

    if(register_io < 18){
        io_mux = pwm_io_mux_value_list[register_io][0];
    }else if(register_io < 28){
        io_mux = pwm_io_mux_value_list[18][0];
    }else if(register_io < 39){
        io_mux = pwm_io_mux_value_list[18+(register_io - 28)][0];
    }else if(register_io < 56){
        io_mux = pwm_io_mux_value_list[24+(register_io - 39)][0];
    }else{
        io_mux = pwm_io_mux_value_list[27+(register_io - 56)][0];
    }

    if(io_mux != 0xffUL){
        GPIO_MuxConfig(register_io,io_mux,GPIO_PIN_TYPE_STD,GPIO_QUAL_SYNC);
    }
}

static rt_err_t qx_hw_pwm_init(struct qx_pwm* instance)
{
    uint32_t pwm_base = instance->channel_regbase;

    /* no actually output channel */
    if(instance->output_ioA == 0xffUL && instance->output_ioB == 0xffUL){
        return RT_ERROR;
    }

    /* ioA check */
    if(instance->output_ioA != 0xffUL){
        if(qx_pwm_support_io_check(instance->output_ioA) != RT_EOK){
            return RT_ERROR;
        }

        qx_pwm_io_config(instance->output_ioA);
    }

    /* ioB check */
    if(instance->output_ioB != 0xffUL){
        if(qx_pwm_support_io_check(instance->output_ioB) != RT_EOK){
            return RT_ERROR;
        }

        qx_pwm_io_config(instance->output_ioB);
    }

#if 0
    /* init pwm timer, pwm output mode */
    SysCtl_enablePeripheral(instance->clk_source);

    EPWM_setClockPrescaler(pwm_base, EPWM_CLOCK_DIVIDER_4, EPWM_HSCLOCK_DIVIDER_10); // 2.5M
    EPWM_setTimeBasePeriod(pwm_base, PWM_ARR(__pwm_freq) - 1);
    EPWM_setTimeBaseCounter(pwm_base, 0);
    EPWM_setTimeBaseCounterMode(pwm_base, EPWM_COUNTER_MODE_UP);

    EPWM_disablePhaseShiftLoad(pwm_base);
    EPWM_setPhaseShift(pwm_base, 0);

    /* 全局加载 */
    EPWM_enableGlobalLoadRegisters(pwm_base, EPWM_GL_REGISTER_TBPRD_TBPRDHR 
                                            | EPWM_GL_REGISTER_CMPA_CMPAHR
                                            | EPWM_GL_REGISTER_CMPB_CMPBHR
                                            | EPWM_GL_REGISTER_AQCTLA_AQCTLA2
                                            | EPWM_GL_REGISTER_AQCTLB_AQCTLB2);

    EPWM_setGlobalLoadTrigger(pwm_base, EPWM_GL_LOAD_PULSE_GLOBAL_FORCE);
    EPWM_setGlobalLoadEventPrescale(pwm_base, 1); // Generate strobe on GLDCNT = 001 (1st event)
    EPWM_enableGlobalLoad(pwm_base);

    /* EPWMA channel */
    EPWM_setCounterCompareValue(pwm_base, EPWM_COUNTER_COMPARE_A, 0);
    EPWM_setCounterCompareShadowLoadMode(pwm_base, EPWM_COUNTER_COMPARE_A, EPWM_COMP_LOAD_ON_CNTR_PERIOD);

    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_HIGH,      EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_LOW,       EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPB);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_A, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);

    /* EPWMB channel */
    EPWM_setCounterCompareValue(pwm_base, EPWM_COUNTER_COMPARE_B, 0);
    EPWM_setCounterCompareShadowLoadMode(pwm_base, EPWM_COUNTER_COMPARE_B, EPWM_COMP_LOAD_ON_CNTR_PERIOD);

    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_HIGH,      EPWM_AQ_OUTPUT_ON_TIMEBASE_ZERO);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_PERIOD);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPA);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPA);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_LOW,       EPWM_AQ_OUTPUT_ON_TIMEBASE_UP_CMPB);
    EPWM_setActionQualifierAction(pwm_base, EPWM_AQ_OUTPUT_B, EPWM_AQ_OUTPUT_NO_CHANGE, EPWM_AQ_OUTPUT_ON_TIMEBASE_DOWN_CMPB);
#endif

    return RT_EOK;
}

/*****************************************************************************************************/
static void pwm_timer_init(void)
{
    int i;

    uint32_t pwm_obj_cnt = sizeof(qx_pwm_obj) / sizeof(qx_pwm_obj[0]);

    SysCtl_disablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);

    for (i = 0; i < pwm_obj_cnt; i++) {
        qx_hw_pwm_init(&qx_pwm_obj[i]);
    }

    /* Enable sync and clock to PWM */
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_TBCLKSYNC);
}

rt_inline void __read_pwm(uint8_t chan_id, float* dc)
{
    *dc = __pwm_dc[chan_id];
}

static inline void timer_channel_output_pulse_value_config(uint32_t base, EPWM_CounterCompareModule compModule, uint16_t compCount)
{   
    // EPWM_setTimeBasePeriod(base, PWM_ARR(__pwm_freq) - 1);
    // EPWM_setCounterCompareValue(base, compModule, compCount);
    // EPWM_setCounterCompareShadowLoadMode(base, compModule, EPWM_COMP_LOAD_ON_CNTR_PERIOD);

    // EPWM_forceGlobalLoadOneShotEvent(base);
}

rt_inline void __write_pwm(uint8_t chan_id, float dc)
{
    switch (chan_id) {
    case 0:
        timer_channel_output_pulse_value_config(EPWM1_BASE, EPWM_COUNTER_COMPARE_A, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 1:
        timer_channel_output_pulse_value_config(EPWM1_BASE, EPWM_COUNTER_COMPARE_B, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 2:
        timer_channel_output_pulse_value_config(EPWM2_BASE, EPWM_COUNTER_COMPARE_A, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 3:
        timer_channel_output_pulse_value_config(EPWM2_BASE, EPWM_COUNTER_COMPARE_B, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 4:
        timer_channel_output_pulse_value_config(EPWM3_BASE, EPWM_COUNTER_COMPARE_A, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 5:
        timer_channel_output_pulse_value_config(EPWM3_BASE, EPWM_COUNTER_COMPARE_B, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 6:
        timer_channel_output_pulse_value_config(EPWM4_BASE, EPWM_COUNTER_COMPARE_A, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 7:
        timer_channel_output_pulse_value_config(EPWM4_BASE, EPWM_COUNTER_COMPARE_B, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 8:
        timer_channel_output_pulse_value_config(EPWM5_BASE, EPWM_COUNTER_COMPARE_A, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    case 9:
        timer_channel_output_pulse_value_config(EPWM5_BASE, EPWM_COUNTER_COMPARE_B, PWM_ARR(__pwm_freq) * dc - 1);
        break;
    default:
        return;
    }

    __pwm_dc[chan_id] = dc;
}

static rt_err_t __set_pwm_frequency(uint16_t freq)
{
    if (freq < PWM_FREQ_50HZ || freq > PWM_FREQ_400HZ) {
        /* invalid frequency */
        return RT_EINVAL;
    }

    __pwm_freq = freq;

    /* the timer compare value should be re-configured */
    for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        __write_pwm(i, __pwm_dc[i]);
    }

    return RT_EOK;
}

static rt_err_t pwm_config(actuator_dev_t dev, const struct actuator_configure* cfg)
{
    DRV_DBG("aux out configured: pwm frequency:%d\n", cfg->pwm_config.pwm_freq);

    if (__set_pwm_frequency(cfg->pwm_config.pwm_freq) != RT_EOK) {
        return RT_ERROR;
    }
    /* update device configuration */
    dev->config = *cfg;

    return RT_EOK;
}

static rt_err_t pwm_control(actuator_dev_t dev, int cmd, void* arg)
{
    rt_err_t ret = RT_EOK;
    uint32_t pwm_obj_cnt = sizeof(qx_pwm_obj) / sizeof(qx_pwm_obj[0]);
    uint16_t i;

    switch (cmd) {
    case ACT_CMD_CHANNEL_ENABLE:
        for (i = 0; i < pwm_obj_cnt; i++) {
            SysCtl_enablePeripheral(qx_pwm_obj[i].clk_source);
        }

        /* set to lowest pwm before open */
        for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
            __write_pwm(i, VAL_TO_DC(act_dev.range[0]));
        }
        break;
    case ACT_CMD_CHANNEL_DISABLE:
        for (i = 0; i < pwm_obj_cnt; i++) {
            SysCtl_disablePeripheral(qx_pwm_obj[i].clk_source);
        }
        break;
    case ACT_CMD_SET_PROTOCOL:
        /* TODO: Support dshot */
        ret = RT_EINVAL;
        break;
    default:
        ret = RT_EINVAL;
        break;
    }

    return ret;
}

static rt_size_t pwm_read(actuator_dev_t dev, rt_uint16_t chan_sel, rt_uint16_t* chan_val, rt_size_t size)
{
    rt_uint16_t* index = chan_val;
    float dc;

    for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        if (chan_sel & (1 << i)) {
            __read_pwm(i, &dc);
            *index = DC_TO_VAL(dc);
            index++;
        }
    }

    return size;
}

static rt_size_t pwm_write(actuator_dev_t dev, rt_uint16_t chan_sel, const rt_uint16_t* chan_val, rt_size_t size)
{
    const rt_uint16_t* index = chan_val;
    rt_uint16_t val;
    float dc;

    for (uint8_t i = 0; i < MAX_PWM_OUT_CHAN; i++) {
        if (chan_sel & (1 << i)) {
            val = *index;
            /* calculate pwm duty cycle */
            dc = VAL_TO_DC(val);
            /* update pwm signal */
            __write_pwm(i, dc);

            index++;
        }
    }

    return size;
}

rt_err_t drv_pwm_init(void)
{
    /* init pwm timer, pwm output mode */
    pwm_timer_init();

    /* register actuator hal device */
    return hal_actuator_register(&act_dev, "main_out", RT_DEVICE_FLAG_RDWR, NULL);
}

